#include <memory>
#include <rclcpp/rclcpp.hpp>

#include <asio_driver/device/joystick.hpp>
#include <asio_driver/device/joystick_layout.hpp>

#include <general_interface/msg/chassis_servo.hpp>
#include <general_interface/msg/control_signal.hpp>
#include <general_interface/srv/mode_selection.hpp>

// joystick Test: https://www.9slab.com/gamepad/home

class JoystickNode : public rclcpp::Node
{
  private:
    using ModeSelection = general_interface::srv::ModeSelection;
    AD::AsioContext::SharePtr context;
    std::unique_ptr<AD::Joystick> js;
    rclcpp::Publisher<general_interface::msg::ChassisServo>::SharedPtr pub1;
    rclcpp::Publisher<general_interface::msg::ChassisServo>::SharedPtr pub2;
    rclcpp::Client<ModeSelection>::SharedPtr client;
    rclcpp::TimerBase::SharedPtr timerTick;

    double fs = 0, rs = 0;
    bool speedMode = true;

  public:
    JoystickNode() : Node("joystick_node")
    {
        context = AD::AsioContext::CreatAsioContext();
        js = std::make_unique<AD::Joystick>(context);
        if (!js->Open("/dev/input/js0"))
        {
            RCLCPP_ERROR(get_logger(), "Failed to open joystick");
        }

        declare_parameter("forward_scale", 30.0 / 180 * std::numbers::pi);
        declare_parameter("rotate_scale", 10.0 / 180 * std::numbers::pi);
        declare_parameter("mode", "speed");
        if (!has_parameter("mode"))
            throw std::runtime_error(
                "No mode parameter, check update of js to chassis_servo");
        fs = get_parameter("forward_scale").as_double();
        rs = get_parameter("rotate_scale").as_double();
        speedMode = get_parameter("mode").as_string() == "speed";
        if (speedMode)
            RCLCPP_INFO(get_logger(), "Speed mode");

        pub1 = create_publisher<general_interface::msg::ChassisServo>(
            "control_signal_1", 10);
        pub2 = create_publisher<general_interface::msg::ChassisServo>(
            "control_signal_2", 10);
        client = create_client<ModeSelection>("auto_mode", 10);
        timerTick = create_wall_timer(std::chrono::milliseconds(1),
                                      std::bind(&JoystickNode::OnTick, this));
    }

  private:
    void OnTick()
    {
        context->SpinOnce();
        if (!js->HasNewData())
        {
            return;
        }
        auto frame = js->ReadFrame();
        float rotate1 = frame.GetAix(AD::AsCode(AD::XBoxAixLayout::LeftHor));
        float forward1 = frame.GetAix(AD::AsCode(AD::XBoxAixLayout::LeftVec));

        float rotate2 = frame.GetAix(AD::AsCode(3));
        float forward2 = frame.GetAix(AD::AsCode(4));

        general_interface::msg::ChassisServo msg;
        if (speedMode)
        {
            msg.speed_gain = 1.0;
            msg.speed = -forward1 * fs;
        }
        else
        {
            msg.angle_gain = 1.0;
            msg.angle = -forward1 * fs;
        }
        msg.gyroz = rotate1 * rs;
        pub1->publish(msg);
        if (speedMode)
            msg.speed = -forward2 * fs;
        else
            msg.angle = -forward2 * fs;

        msg.gyroz = rotate2 * rs;
        pub2->publish(msg);

        if (frame.GetButton(AD::AsCode(AD::XBoxButtonLayout::Y)))
        {
            auto request = std::make_shared<
                general_interface::srv::ModeSelection::Request>();
            request->mode =
                general_interface::srv::ModeSelection::Request::AUTO;
            auto future = client->async_send_request(request);
            if (rclcpp::spin_until_future_complete(
                    this->shared_from_this(), future,
                    std::chrono::milliseconds(100)) ==
                rclcpp::FutureReturnCode::SUCCESS)
            {
                auto response = future.get();
                RCLCPP_INFO(get_logger(), "Switch to auto mode: %d",
                            response->success);
            }
            else
            {
                RCLCPP_ERROR(get_logger(), "Failed to switch mode");
            }
        }
        else if (frame.GetButton(AD::AsCode(AD::XBoxButtonLayout::A)))
        {
            auto request = std::make_shared<
                general_interface::srv::ModeSelection::Request>();
            request->mode =
                general_interface::srv::ModeSelection::Request::MANUAL;
            auto future = client->async_send_request(request);
            if (rclcpp::spin_until_future_complete(
                    this->shared_from_this(), future,
                    std::chrono::milliseconds(100)) ==
                rclcpp::FutureReturnCode::SUCCESS)
            {
                auto response = future.get();
                RCLCPP_INFO(get_logger(), "Switch to manual mode: %d",
                            response->success);
            }
            else
            {
                RCLCPP_ERROR(get_logger(), "Failed to switch mode");
            }
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<JoystickNode>());
    rclcpp::shutdown();
    return 0;
}